#include "obstacle_detector/obstacle_detector_nodelet.h"

namespace obstacle_detector
{
void ObstacleDetectorNodelet::onInit()
{
	init_nodelet();
	CREATE_MONITOR_PUBLISHER(wait_for_monitor("obstacle_detector"), 1);

	bool debug=boost::lexical_cast<bool>(params_["debug"]);

	obstacle_detector_=std::make_unique<ObstacleDetector>(params_["calib_folder"],
		params_["weight_folder"],debug);

	debug_logger_=std::make_shared<logger::Logger>("obstacle_detector_nodelet");

	sub_1_.subscribe(nh_, params_["left_lidar_topic"], 1);
	sub_2_.subscribe(nh_, params_["right_lidar_topic"], 1);

	pc_sync_.reset(new PcSync(PointcloudPolicy(10), sub_1_, sub_2_));
	pc_sync_->registerCallback(&ObstacleDetectorNodelet::pointcloud_callback, this);

	CREATE_PUBLISHER<qomolo_msgs::SpeedLimit>("speed_limit","speed_limit",1,false);
	CREATE_PUBLISHER<visualization_msgs::MarkerArray>("obstacle_visual","/obstacle_visual2",1,false);
	CREATE_PUBLISHER<qomolo_msgs::ObstacleArray>("obstacles","/obstacles",1,false);

	CREATE_SUBSCRIPION("/filtered_pointcloud",
		&ObstacleDetectorNodelet::filterpc_callback,1);
	CREATE_SUBSCRIPION("/lanelet2_map",
		&ObstacleDetectorNodelet::map_callback,10);
	CREATE_SUBSCRIPION("/fusion_odom",
		&ObstacleDetectorNodelet::odom_callback,10);
	CREATE_SUBSCRIPION("/vehicle_param",
		&ObstacleDetectorNodelet::vehicleparam_callback,10);
	CREATE_SUBSCRIPION("/control_sequence",
		&ObstacleDetectorNodelet::control_sequence_callback,10);
}

void ObstacleDetectorNodelet::pointcloud_callback(
	const sensor_msgs::PointCloud2::ConstPtr& pt2_msg1,
	const sensor_msgs::PointCloud2::ConstPtr& pt2_msg2)
{
	monitor_info_publish(0);
	module_base::Location location;
	location.stamp=pt2_msg1->header.stamp.toSec();
	bool loc_found=find_nearest_loc(location);
	if (!loc_found)
	{
		printf ("loc not found\n");
		return;
	}

	// printf ("get data\n");
	auto start = std::chrono::system_clock::now();
	double pt1_stamp=pt2_msg1->header.stamp.toSec();
	double pt2_stamp=pt2_msg2->header.stamp.toSec();
	double stamp=pt1_stamp>pt2_stamp ? pt1_stamp : pt2_stamp;
	// printf ("pc stamp: %f\n",stamp);
	int point_num1=pt2_msg1->width * pt2_msg1->height;
	int point_num2=pt2_msg2->width * pt2_msg2->height;
	obstacle_detector_->set_pcdata(pt2_msg1->data.data(),pt2_msg2->data.data(),
		point_num1,point_num2,stamp);
	float collision_index=obstacle_detector_->check_collision(location);
	// printf ("collision_index: %f\n",collision_index);
	publish_speed_limit(collision_index);
	publish_obstacles(location);

	// auto end   = std::chrono::system_clock::now();
	// auto duration = std::chrono::duration_cast<ChronoMicro>(end - start);
	// printf("process Time : %f s\n",
	// 	double(duration.count()) * ChronoMicro::period::num / ChronoMicro::period::den);
}

void ObstacleDetectorNodelet::filterpc_callback(
	const sensor_msgs::PointCloud2::ConstPtr& pc_msg)
{
	double stamp=pc_msg->header.stamp.toSec();
	// printf ("*************************get filtered data: %f\n",stamp);
	int point_num=pc_msg->width * pc_msg->height;
	obstacle_detector_->set_filter_pcdata(pc_msg->data.data(),point_num);
}

void ObstacleDetectorNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr odom_msg)
{
	loc_mutex_.lock();
	module_base::Location odom_loc;
	odom_loc.stamp=odom_msg->header.stamp.toSec();
	odom_loc.yaw=tf::getYaw(odom_msg->pose.pose.orientation);
	odom_loc.x=odom_msg->pose.pose.position.x;
	odom_loc.y=odom_msg->pose.pose.position.y;
	odom_loc_list_.push_back(odom_loc);
	// printf ("loc yaw: %f\n",odom_loc.yaw,odom_loc.x,odom_loc.y);
	if (odom_loc_list_.size()>20)
	{
		odom_loc_list_.erase(odom_loc_list_.begin());
	}
	loc_mutex_.unlock();
}

bool ObstacleDetectorNodelet::find_nearest_loc(module_base::Location &loc)
{
	loc_mutex_.lock();
	float min_diff=1;
	int min_index=-1;
	for (int i=0;i<odom_loc_list_.size();i++)
	{
		float time_diff=fabs(loc.stamp - odom_loc_list_[i].stamp);
		if (time_diff<min_diff)
		{
			min_diff=time_diff;
			min_index=i;
		}
	}

	// printf ("min_index: %d\n",min_index);

	if (min_index==-1)
	{
		loc_mutex_.unlock();
		return false;
	}
	
	loc=odom_loc_list_[min_index];

	loc_mutex_.unlock();
	return true;
}

void ObstacleDetectorNodelet::publish_obstacles(
	const module_base::Location &loc)
{
	std::vector<Object> obstacles=obstacle_detector_->get_obstacles();
	publish_obstacles_visual(obstacles);
	qomolo_msgs::ObstacleArray obstacle_msg;
	obstacle_msg.header.stamp=ros::Time(loc.stamp);
	obstacle_msg.header.frame_id="map";
	obstacle_msg.loc_x=loc.x;
	obstacle_msg.loc_y=loc.y;
	obstacle_msg.loc_yaw=loc.yaw;
	for (int i=0;i<obstacles.size();i++)
	{
		qomolo_msgs::Obstacle obstacle;
		obstacle.label=obstacles[i].label;
		obstacle.bbox.points.resize(4);
		for (int j=0;j<4;j++)
		{
			obstacle.bbox.points[j].x=obstacles[i].global_bbox(0,j);
			obstacle.bbox.points[j].y=obstacles[i].global_bbox(1,j);
			obstacle.bbox.points[j].z=0;
		}
		if (obstacles[i].predict_locs.cols()>1)
		{
			for (int j=0;j<obstacles[i].predict_locs.cols();j++)
			{
				if (j%5!=0) continue;
				geometry_msgs::Point new_point;
				new_point.x=obstacles[i].predict_locs(0,j);
				new_point.y=obstacles[i].predict_locs(1,j);
				new_point.z=0;
				obstacle.predict_locs.points.push_back(new_point);
			}
		}
		obstacle_msg.obstacles.push_back(obstacle);
	}
	publishers_["obstacles"].publish(obstacle_msg);

}

void ObstacleDetectorNodelet::publish_obstacles_visual(
	const std::vector<Object> &obstacles)
{
	visualization_msgs::MarkerArray visual_msg;
	visual_msg.markers.clear();
	for (int i=0;i<obstacles.size();i++)
	{
		float color[3];
		get_color(obstacles[i].label,color);
		// printf ("label color: %d,%f,%f,%f\n",i,color[0],color[1],color[2]);
		visualization_msgs::Marker obstacle_marker;
		obstacle_marker.header.stamp=ros::Time::now();
		obstacle_marker.header.frame_id="map";
		obstacle_marker.ns="obstacle_"+std::to_string(i);
		obstacle_marker.type=5;
		obstacle_marker.color.r=color[0];
		obstacle_marker.color.g=color[1];
		obstacle_marker.color.b=color[2];
		obstacle_marker.color.a=1;
		obstacle_marker.action=0;
		obstacle_marker.pose.orientation.w=1;
		obstacle_marker.scale.x = 0.1;
		for (int j=0;j<4;j++)
		{
			geometry_msgs::Point point1;
			point1.x=obstacles[i].global_bbox(0,j);
			point1.y=obstacles[i].global_bbox(1,j);
			point1.z=0;
			geometry_msgs::Point point2;
			point2.x=obstacles[i].global_bbox(0,(j+1)%4);
			point2.y=obstacles[i].global_bbox(1,(j+1)%4);
			point2.z=0;
			obstacle_marker.points.push_back(point1);
			obstacle_marker.points.push_back(point2);
		}
		if (obstacles[i].predict_locs.cols()>1)
		{
			geometry_msgs::Point lst_point;
			lst_point.x=obstacles[i].predict_locs(0,0);
			lst_point.y=obstacles[i].predict_locs(1,0);
			lst_point.z=0;
			for (int j=1;j<obstacles[i].predict_locs.cols();j++)
			{
				if (j%5!=0) continue;
				geometry_msgs::Point new_point;
				new_point.x=obstacles[i].predict_locs(0,j);
				new_point.y=obstacles[i].predict_locs(1,j);
				new_point.z=0;
				obstacle_marker.points.push_back(lst_point);
				obstacle_marker.points.push_back(new_point);
				lst_point=new_point;
			}
		}
		visual_msg.markers.push_back(obstacle_marker);
	}
	publishers_["obstacle_visual"].publish(visual_msg);
}

void ObstacleDetectorNodelet::publish_speed_limit(float collision_index)
{
	if (collision_index>=0)
	{
		warning_publish(34,"obstacle_detector: "+std::to_string(collision_index));
		debug_logger_->write_log("obstacle_motion: ",collision_index,current_speed_);
		qomolo_msgs::SpeedLimit speed_limit_msg;
		speed_limit_msg.header.frame_id="map";
		speed_limit_msg.module_name="obstacle_detector";
		if (collision_index<=5) //预测在5秒内会发生碰撞   空车状态下从7.6减速到0 需要4s
		{
			speed_limit_msg.speed_limit=0;
		}
		else if (collision_index<=7) //空车状态下从7.6减速到1 需要6s
		{
			float speed=std::max(1.0,0.5*current_speed_);
			speed_limit_msg.speed_limit=speed;
		}
		else if (collision_index<=10) //空车状态下从7.6减速到2 需要6s
		{
			float speed=std::max(1.0,0.7*current_speed_);
			speed_limit_msg.speed_limit=speed;
		}
		else //空车状态下从7.6减速到3 需要6s
		{
			float speed=std::max(1.0,0.9*current_speed_);
			speed_limit_msg.speed_limit=speed;
		}
		for (int i=0;i<5;i++)
		{
			speed_limit_msg.header.stamp=ros::Time::now();
			publishers_["speed_limit"].publish(speed_limit_msg);
			usleep(10000);
		}
	}
}

void ObstacleDetectorNodelet::vehicleparam_callback(const std_msgs::String::ConstPtr& map_msg)
{
	if (!parse_vehicle_param(map_msg->data))
	{
		printf ("vehicle_param load fail\n");
		debug_logger_->write_log("vehicle_param load fail");
		exit(-1);
	}
	obstacle_detector_->set_vechile_param(vehicle_param_);
}

void ObstacleDetectorNodelet::control_sequence_callback(
	const qomolo_msgs::ControlSequence::ConstPtr control_sequence_msg)
{
	// printf ("get control sequence\n");
	current_speed_=control_sequence_msg->velocity;
	float control_sequence[51*3];
	control_sequence[0]=control_sequence_msg->velocity;
	control_sequence[1]=control_sequence_msg->steer1;
	control_sequence[2]=control_sequence_msg->steer2;
	for (int i=0;i<control_sequence_msg->control_sequence.size();i++)
	{
		control_sequence[(i+1)*3+0]=control_sequence_msg->control_sequence[i].acce;
		control_sequence[(i+1)*3+1]=control_sequence_msg->control_sequence[i].steer1;
		control_sequence[(i+1)*3+2]=control_sequence_msg->control_sequence[i].steer2;
	}
	obstacle_detector_->set_control_sequence(control_sequence);
}


void ObstacleDetectorNodelet::map_callback(const std_msgs::String::ConstPtr& map_msg)
{
	obstacle_detector_->set_map(map_msg->data);
}

}//namespace

PLUGINLIB_EXPORT_CLASS(obstacle_detector::ObstacleDetectorNodelet, nodelet::Nodelet)